#include "GameCore/Renderer.h"
#include "PhysicsDebugDrawer.h"
#include "PhysicsUtils.h"

using namespace core;
using namespace cmn;

PhysicsDebugDrawer::PhysicsDebugDrawer( const Color& borderColor, const Color& fillColor ) :
	m_borderColor( borderColor ),
	m_fillColor( fillColor )
{}

void PhysicsDebugDrawer::DrawBody( const b2Body* body ) const {
	Transform bodyPos = ToTransform( body->GetTransform() );

	const b2Fixture* fixture = body->GetFixtureList();
	while( fixture ) {
		const b2Shape* shape = fixture->GetShape();
		DrawShape( bodyPos, shape );
		fixture = fixture->GetNext();
	}
}

void PhysicsDebugDrawer::DrawShape( const Transform& pos, const b2Shape* shape ) const {
	const b2CircleShape* circle = dynamic_cast<const b2CircleShape*>( shape );
	if( circle )
		DrawCircle( pos, circle );

	const b2PolygonShape* polygon = dynamic_cast<const b2PolygonShape*>( shape );
	if( polygon )
		DrawPolygon( pos, polygon );
}

void PhysicsDebugDrawer::DrawCircle( const Transform& pos, const b2CircleShape* circle ) const {
	float radius = circle->m_radius;
	b2Vec2 b2Center = circle->m_p;
	Vec2 center = Vec2( b2Center.x, b2Center.y );
	Vec2 worldCenter = pos * center;

	Renderer::DrawFilledCircle( Vec3( worldCenter.X, worldCenter.Y, 0.0f ), radius, m_borderColor, m_fillColor );
}

void PhysicsDebugDrawer::DrawPolygon( const Transform& pos, const b2PolygonShape* polygon ) const {
	Vector<Vec3>* vertices3 = new Vector<Vec3>();

	int count =  polygon->GetVertexCount();
	for( int i = 0; i < count; ++i ) {
		b2Vec2 b2Vertex = polygon->GetVertex( i );
		Vec2 vertex = Vec2( b2Vertex.x, b2Vertex.y );
		Vec2 vec2 = pos * vertex;
		vertices3->Add( Vec3( vec2.X, vec2.Y, 0.0f ) );
	}
	Renderer::DrawFilledPolygon( vertices3, m_borderColor, m_fillColor );
	delete vertices3;
}
